#include "outline/bezier.h"

namespace outline {

template <class R>
void bezier<R>::coefficients(real_type t, real_type r[4])
{
	real_type t2 = t*t;
	real_type u = 1-t;
	real_type u2 = u*u;
	real_type t3 = t2*t;
	real_type u3 = u2*u;
	r[0] = u3;
	r[1] = 3*t*u2;
	r[2] = 3*t2*u;
	r[3] = t3;
}

template <class R>
void bezier<R>::value_n_deriv(real_type t, 
	real_point_type * value, real_point_type * deriv,
	real_type * r1, real_type * r2) const
{
	real_type t2 = t*t;
	real_type u = 1-t;
	real_type u2 = u*u;
	if (value) {
		real_type t3 = t2*t;
		real_type u3 = u2*u;
		* value = 
			u3 * start() + 
			3*t*u2 * start_control() +
			3*t2*u * finish_control() +
			t3 * finish();
	}
	if (deriv) {
		* deriv = 
			3*u2 * (start_control() - start()) +
			6*t*u * (finish_control() - start_control()) +
			3*t2 * (finish() - finish_control());
	}
	if (r1)
		*r1 = 3*t*u2;
	if (r2)
		*r2 = 3*t2*u;
}

template <class R>
typename bezier<R>::real_type  bezier<R>::sample(real_point_vector_type & s) const
{
	uint n = s.size();
	assert (n >= 2);
	s[0] = start();
	s[n-1] = finish();
	if (n == 2)
		return 1;

	real_type dt = real_type(1.0) / (n - 1);
	real_type t = 0;
	for (uint i = 1; i < n-1; ++i) {
		t += dt;
		s[i] = value(t);
	}
	return dt;
}

template <class R>
typename bezier<R>::real_type bezier<R>::sample_lengths(real_vector_type & s, bool normalize) const
{
	real_point_vector_type p(s.size());
	real_type dt = sample(p);
	outline::length(p, s, normalize);
	return dt;
}

template <class R>
typename bezier<R>::real_type bezier<R>::length(unsigned subdiv) const
{
	real_point_vector_type s(subdiv);
	sample(s);
	return outline::length(s);
}

template <class R>
void bezier<R>::length_to_parameter(real_vector_type & s, unsigned subdiv) const
{
	real_vector_type l(subdiv);
	real_type dt = sample_lengths(l, true);
	uint i = 0, j;
	real_type t = 0;

	//check input contract
	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		assert(v >= 0 && v <= 1);
		assert(j == 0 || s[j] >= s[j-1]);
	}

	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		if (v == 0 || v == 1)
			continue; //0 maps to 0 and 1 maps to 1
		while (l[i+1] < v) {
			++i;
			t += dt;
			assert(i+1 < subdiv);
		}
		assert (v >= l[i]);
		v = t + (v - l[i]) / (l[i+1] - l[i]) * dt;
	}

	//check output contract
	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		assert(v >= 0 && v <= 1);
		assert(j == 0 || s[j] >= s[j-1]);
	}
}

template <class R>
void bezier<R>::parameter_to_length(real_vector_type & s, unsigned subdiv) const
{
	real_vector_type l(subdiv);
	real_type dt = sample_lengths(l, true);
	uint i = 0, j;
	real_type t = 0;

	//check input contract
	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		assert(v >= 0 && v <= 1);
		assert(j == 0 || s[j] >= s[j-1]);
	}

	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		if (v == 0 || v == 1)
			continue; //0 maps to 0 and 1 maps to 1
		while (t + dt < v) {
			++i;
			t += dt;
			assert(i+1 < subdiv);
		}
		assert (v >= t);
		v = l[i] + (v - t) / dt * (l[i+1] - l[i]);
	}

	//check output contract
	for (j = 0; j < s.size(); ++j) {
		real_type & v = s[j];
		assert(v >= 0 && v <= 1);
		assert(j == 0 || s[j] >= s[j-1]);
	}
}

template <class R>
R bezier<R>::length_to_parameter(real_type l, unsigned subdiv) const
{
	real_vector_type s(1, l);
	length_to_parameter(s, subdiv);
	return s[0];
}

template <class R>
R bezier<R>::parameter_to_length(real_type p, unsigned subdiv) const
{
	real_vector_type s(1, p);
	parameter_to_length(s, subdiv);
	return s[0];
}

template bezier<float>;
template bezier<double>;

} //namespace outline
